#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"

using namespace std::chrono_literals;

class TF_Listener : public rclcpp::Node
{
private:
    std::shared_ptr<tf2_ros::TransformListener> listener_;
    std::shared_ptr<tf2_ros::Buffer> buffer_;
    rclcpp::TimerBase::SharedPtr time_;

    void dotime_CallBack()
    {
        try
        {
            auto listener = buffer_->lookupTransform("map", "base_footprint", tf2::TimePointZero, 5s);
            RCLCPP_INFO(this->get_logger(),
                        "父坐标系：%s,子坐标系：%s,偏移量（%.2f,%.2f,%.2f)",
                        listener.header.frame_id.c_str(),
                        listener.child_frame_id.c_str(),
                        listener.transform.translation.x,
                        listener.transform.translation.y,
                        listener.transform.translation.z);
        }
        catch (const tf2::LookupException &e)
        {
            RCLCPP_ERROR(this->get_logger(), "异常提示%s", e.what());
        }
    }

public:
    TF_Listener() : Node("nav2_robot_tf_listener_node")
    {
        buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
        listener_ = std::make_shared<tf2_ros::TransformListener>(*buffer_, this);
        time_ = this->create_wall_timer(1s, std::bind(&TF_Listener::dotime_CallBack, this));
    }
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<TF_Listener>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}
